#include "StdAfx.h"
#include "PathFinding.h"
#include "EmptyApp.h"

PathFinding::PathFinding(Level* level) : m_pLevel(level) 
{
	m_initializedStartGoal = false;
	m_foundGoal = false;
}

PathFinding::~PathFinding(void)
{

}

void PathFinding::FindPath(D3DXVECTOR3 currentPos, D3DXVECTOR3 targetPos)
{
	if(!m_initializedStartGoal)
	{
		for(int i = 0; i < m_openList.size(); ++i)
		{
			delete m_openList[i];
		}
		m_openList.clear();

		for(int i = 0; i < m_VisitedList.size(); ++i)
		{
			delete m_VisitedList[i];
		}
		m_VisitedList.clear();

		for(int i = 0; i < m_pathToGoal.size(); ++i)
		{
			delete m_pathToGoal[i];
		}
		m_pathToGoal.clear();

		//initialize start
		SearchCell start;
		start.m_xcoord = currentPos.x / CELLSIZE;
		start.m_zcoord = currentPos.z / CELLSIZE;

		//innitialize goal
		SearchCell goal;
		goal.m_xcoord = targetPos.x / CELLSIZE;
		goal.m_zcoord = targetPos.z / CELLSIZE;

		setStartAndGoal(start, goal);

		m_initializedStartGoal = true;
	}

	if(m_initializedStartGoal)
	{
		ContinuePath();
	}
}

void PathFinding::setStartAndGoal(SearchCell start, SearchCell goal)
{
	m_StartCell = new SearchCell(start.m_xcoord, start.m_zcoord, NULL);
	m_GoalCell = new SearchCell(goal.m_xcoord, goal.m_zcoord, &goal);

	m_StartCell->m_G = 0;
	m_StartCell->m_H = m_StartCell->ManHattanDistance(m_GoalCell);
	m_StartCell->m_parent = 0;

	m_openList.push_back(m_StartCell);
}

SearchCell* PathFinding::GetNextCell()
{
	float bestF = 999999.0f;
	int cellIndex = -1;
	SearchCell* nextCell = NULL;

	for(int i = 0; i < m_openList.size(); ++i)
	{
		if(m_openList[i]->GetF() < bestF)
		{
			bestF = m_openList[i]->GetF();
			cellIndex = i;
		}
	}

	if(cellIndex >= 0)
	{
		nextCell = m_openList[cellIndex];
		m_VisitedList.push_back(nextCell);
		m_openList.erase(m_openList.begin() + cellIndex);
	}

	return nextCell;
}

void PathFinding::Pathopened(int x, int z, float newCost, SearchCell *parent)
{

	// Note : I know that using Raycast for pathfinding is very heavy and not a good Idea.
	// I need to change the code so that a a matrix of cells is made befor the start of the game, and use this for pathfinding.
	NxRay Raycast;

	Raycast.orig = NxVec3(x * CELLSIZE, 20, z* CELLSIZE);
	Raycast.dir = NxVec3(0, -1, 0);
	Raycast.dir.normalize();

	NxRaycastHit GetIntercect;

	NxShape* ClosestShape = m_pLevel->GetPhysicsScene()->raycastClosestShape(Raycast, NX_ALL_SHAPES, GetIntercect);

	if(ClosestShape)
	{
		if(GetIntercect.worldImpact.y > 4) 
		{
			return;
		}
	}

	int id = z * WORLD_SIZE + x;

	for(int i = 0; i < m_VisitedList.size(); ++i)
	{
		if(id == m_VisitedList[i]->m_id)
		{
			return;
		}	
	}

	SearchCell* newChild = new SearchCell(x, z, parent);
	newChild->m_G = newCost;
	newChild->m_H = parent->ManHattanDistance(m_GoalCell);

	for(int i = 0; i < m_openList.size(); ++i)
	{
		if(id == m_openList[i]->m_id)
		{
			float newF = newChild->m_G + newCost + m_openList[i]->m_H;

			if(m_openList[i]->GetF() > newF)
			{
				m_openList[i]->m_G = newChild->m_G + newCost;
				m_openList[i]->m_parent = newChild;
			}

			else
			{
				delete newChild;
				return;
			}
		}		
	}

	m_openList.push_back(newChild);
}

void PathFinding::ContinuePath()
{
	if(m_openList.empty())
	{
		return;
	}

	SearchCell* currentCell = GetNextCell();

	if(currentCell->m_id == m_GoalCell->m_id)
	{
		m_GoalCell->m_parent = currentCell->m_parent;

		SearchCell* getPath;

		for(getPath = m_GoalCell; getPath != NULL; getPath = getPath->m_parent)
		{
			m_pathToGoal.push_back(new D3DXVECTOR3(getPath->m_xcoord * CELLSIZE, 0, getPath->m_zcoord * CELLSIZE));
		}

		m_foundGoal = true;
		return;
	}
	else
	{
		//rightcell
		Pathopened(currentCell->m_xcoord +1, currentCell->m_zcoord, currentCell->m_G +1, currentCell);
		//leftside
		Pathopened(currentCell->m_xcoord -1, currentCell->m_zcoord, currentCell->m_G +1, currentCell);
		//up
		Pathopened(currentCell->m_xcoord , currentCell->m_zcoord + 1, currentCell->m_G +1, currentCell);
		//down
		Pathopened(currentCell->m_xcoord , currentCell->m_zcoord -1, currentCell->m_G +1, currentCell);
		//left-up
		Pathopened(currentCell->m_xcoord - 1, currentCell->m_zcoord + 1, currentCell->m_G + 1.414f, currentCell);
		//left-down
		Pathopened(currentCell->m_xcoord - 1, currentCell->m_zcoord - 1, currentCell->m_G + 1.414f, currentCell);
		//right-up
		Pathopened(currentCell->m_xcoord + 1, currentCell->m_zcoord + 1, currentCell->m_G + 1.414f, currentCell);
		//lright-down
		Pathopened(currentCell->m_xcoord + 1, currentCell->m_zcoord - 1, currentCell->m_G + 1.414f, currentCell);

		for(int i = 0; i < m_openList.size(); ++i)
		{
			if(currentCell->m_id == m_openList[i]->m_id)
			{
				m_openList.erase(m_openList.begin() + i);
			}
		}
	}
}

NxVec3 PathFinding::NextPathPos(LevelElement &Army) // GameObject add here
{
	int index = 2;

	NxVec3 nextPos;
	nextPos.x = m_pathToGoal[m_pathToGoal.size() - index]->x;
	nextPos.z = m_pathToGoal[m_pathToGoal.size() - index]->z;

	D3DXVECTOR4 pos;
	D3DXVec3Transform(&pos, &D3DXVECTOR3(0, 0, 0), &Army.GetWorldMatrix());

	NxVec3 distance = NxVec3(nextPos.x, 0, nextPos.z) - NxVec3(pos.x, 0, pos.z); // position of object of pathfindinng

	if(index < m_pathToGoal.size())
	{
		float i = distance.magnitude();

		if(distance.magnitude() < 2.0f) //if radius of youre object reached a
		{
			m_pathToGoal.erase(m_pathToGoal.end() - index);
		}
	}

	return nextPos;
}

